/*
	把这个当库使用，不用改
*/

#include "pid.h"

// 定义并初始化 PID 参数变量
Motor_PID_Params motor1_pid_params = MOTOR1_PID_PARAMS;
// 如果有其他电机，也可以继续初始化
// Motor_PID_Params motor2_pid_params = MOTOR2_PID_PARAMS;

float PID_calc(PID_HandleTypeDef *pid, float exp_data, float real_data)
{
    if(pid == NULL)
    {
        return 0.0f;    
    }
    
    // 计算误差
    pid->err[2] = pid->err[1];
    pid->err[1] = pid->err[0];
    pid->err[0] = exp_data - real_data;

    if(pid->mode == PID_MODE_POSITION)
    {
        pid->pOut = pid->param.kp * pid->err[0];
        pid->iOut += pid->param.ki * pid->err[0];
        pid->dOut = pid->param.kd * (pid->err[0] - pid->err[1]);
        
        // 积分限幅
        if(pid->iOutMax != 0.0f)
        {
            pid->iOut = pid_Data_Limit(pid->iOut, -pid->iOutMax, pid->iOutMax);
        }
        
        pid->out = pid->pOut + pid->iOut + pid->dOut;
    }
    else if(pid->mode == PID_MODE_INCREMENTAL)
    {
        pid->pOut = pid->param.kp * (pid->err[0] - pid->err[1]);
        pid->iOut = pid->param.ki * pid->err[0];
        pid->dOut = pid->param.kd * (pid->err[0] - 2 * pid->err[1] + pid->err[2]);

        pid->out += pid->pOut + pid->iOut + pid->dOut;
    }

    // 输出限幅
    if(pid->outMax != 0.0f)
    {
        pid->out = pid_Data_Limit(pid->out, -pid->outMax, pid->outMax);
    }

    return pid->out;
}

float PID_calc_err(PID_HandleTypeDef *pid, float err)
{
    if(pid == NULL)
    {
        return 0.0f;    
    }
    if(pid->mode == PID_MODE_POSITION)
    {
        pid->err3[2] = pid->err3[1];
        pid->err3[1] = pid->err3[0];
        pid->err3[0] = err;

        pid->pOut = pid->kp * pid->err3[0];
        pid->iOut += pid->ki * pid->err3[0];
        pid->dOut = pid->kd * (pid->err3[0] - pid->err3[1]);
        
        if(pid->iOutMax != 0.0f)
        {
            pid->iOut = pid_Data_Limit(pid->iOut, -pid->iOutMax, pid->iOutMax);
        }
        
        pid->out = pid->pOut + pid->iOut + pid->dOut;
        
        if(pid->outMax != 0.0f)
        {
            pid->out = pid_Data_Limit(pid->out, -pid->outMax, pid->outMax);
        }
    }
    else if(pid->mode == PID_MODE_ADD)
    {
        pid->err3[2] = pid->err3[1];
        pid->err3[1] = pid->err3[0];
        pid->err3[0] = err;

        pid->pOut = pid->kp * (pid->err3[0] - pid->err3[1]);
        pid->iOut = pid->ki * pid->err3[0];
        pid->dOut = pid->kd * ((pid->err3[0] - pid->err3[1]) - (pid->err3[1] - pid->err3[2]));

        pid->out += pid->pOut + pid->iOut + pid->dOut ;

        if(pid->outMax != 0.0f)
        {
            pid->out = pid_Data_Limit(pid->out, -pid->outMax, pid->outMax);
        }
    }
    return pid->out;
}

void PID_init(PID_HandleTypeDef *pid, PID_InitTypeDef *PID)
{
    if (pid == NULL || PID == NULL)
    {
        return;
    }
    
    pid->mode = PID->mode;
    pid->kp = PID->kp;
    pid->ki = PID->ki;
    pid->kd = PID->kd;
    pid->iOutMax = PID->iOutMax;
    pid->outMax = PID->outMax;
}

void PID_cleardata(PID_HandleTypeDef *pid)
{
    pid->err3[0] = 0;
    pid->err3[1] = 0;
    pid->err3[2] = 0;
    pid->pOut = 0.0f;
    pid->iOut = 0.0f;
    pid->dOut = 0.0f;
    pid->out = 0.0f;
}

void PID_loadparam(PID_HandleTypeDef *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}

void PID_loadtarget(PID_HandleTypeDef *pid, float target)
{
    pid->set = target;
}

void PID_Init(PID_HandleTypeDef *pid, PID_MODE_TypeDef mode)
{
    if (pid == NULL)
    {
        return;
    }
    pid->mode = mode;
    pid->setPoint = 0.0f;
    pid->err[0] = pid->err[1] = pid->err[2] = 0.0f;
    pid->pOut = pid->iOut = pid->dOut = pid->out = 0.0f;
    pid->iOutMax = 0.0f;
    pid->outMax = 0.0f;
}

void PID_SetParam(PID_HandleTypeDef *pid, PID_ParamTypeDef param)
{
    if (pid == NULL)
    {
        return;
    }
    pid->param = param;
    pid->iOutMax = param.iOutMax;
    pid->outMax = param.outMax;
}

void PID_SetTarget(PID_HandleTypeDef *pid, float target)
{
    if (pid == NULL)
    {
        return;
    }
    pid->setPoint = target;
}

void PID_Clear(PID_HandleTypeDef *pid)
{
    if (pid == NULL)
    {
        return;
    }
    pid->err[0] = pid->err[1] = pid->err[2] = 0.0f;
    pid->pOut = pid->iOut = pid->dOut = pid->out = 0.0f;
}

float PID_Calc(PID_HandleTypeDef *pid, float feedback)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    // 更新误差项
    pid->err[2] = pid->err[1];
    pid->err[1] = pid->err[0];
    pid->err[0] = pid->setPoint - feedback;

    if (pid->mode == PID_MODE_POSITION)
    {
        pid->pOut = pid->param.kp * pid->err[0];
        pid->iOut += pid->param.ki * pid->err[0];
        pid->dOut = pid->param.kd * (pid->err[0] - pid->err[1]);

        // 积分限幅
        if (pid->iOutMax != 0.0f)
        {
            pid->iOut = pid_Data_Limit(pid->iOut, -pid->iOutMax, pid->iOutMax);
        }

        pid->out = pid->pOut + pid->iOut + pid->dOut;
    }
    else if (pid->mode == PID_MODE_INCREMENTAL)
    {
        pid->pOut = pid->param.kp * (pid->err[0] - pid->err[1]);
        pid->iOut = pid->param.ki * pid->err[0];
        pid->dOut = pid->param.kd * (pid->err[0] - 2 * pid->err[1] + pid->err[2]);

        pid->out += pid->pOut + pid->iOut + pid->dOut;
    }

    // 输出限幅
    if (pid->outMax != 0.0f)
    {
        pid->out = pid_Data_Limit(pid->out, -pid->outMax, pid->outMax);
    }

    return pid->out;
}

void PID_InitParams(PID_HandleTypeDef *pid, float kp, float ki, float kd, float iOutMax, float outMax)
{
    if (pid == NULL)
    {
        return;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->iOutMax = iOutMax;
    pid->outMax = outMax;
    pid->setPoint = 0.0f;
    pid->err[0] = pid->err[1] = pid->err[2] = 0.0f;
    pid->pOut = pid->iOut = pid->dOut = pid->out = 0.0f;
}
